clear
clc
close all

plotThings = 0; %set to 1 for stuff to output individual plots, 0 for only final combined plots
linewidth = 3; 


ispBurnoutFlag = 2; % set to 1 (default) to use burnout velocity as the 
% delta V from rocket equation (initial velocity for sim), or 2 as the
% velocity at 1/4 sec after launch, approx burnout time of water

tic
filenames = getStaticTestFilenames('staticTestData','normal');
filenametime = toc;

fprintf('Took %2.1f sec to load filenames\n',filenametime);

fuelmass = 1; %kg

tic
[meanIsp,stdIsp,Isps] = calcIsps(filenames,fuelmass);
Isptime = toc;

fprintf('Took %2.1f sec to calculate Isps\n',Isptime);

nCases = length(Isps);

tic
whandle = waitbar(0,'Simulations Running');
parfor i = 1:nCases
    [dat(i).t,dat(i).x,maxdist(i),maxheight(i),VburnoutInt(i)] = runSim([filenames{i}],plotThings);
    
    [datIsp(i).t,datIsp(i).x,maxdistIsp(i),maxheightIsp(i),VburnoutIsp(i)] = runSimDeltaVIsp(Isps(i),plotThings,ispBurnoutFlag);
%     waitbar(i/nCases,whandle,'Simulations Running');
end

close(whandle);

simTime = toc;

fprintf('Took %3.1f sec to run simulations\n',simTime);


% calc trajectory for mean val of Isp to see where it sits
[meanIsp_t,meanIsp_x,meanIsp_maxdist,~,VburnoutIspMean] = runSimDeltaVIsp(meanIsp,plotThings,ispBurnoutFlag);


% calc trajectory for mean val of Isp to see where it sits
[thermo_t,thermo_x,thermo_maxdist,thermo_maxheight,Vburnoutthermo,thrust_thermo] = runSimThermo(plotThings);

%calculate means and standard deviations
meanDist = mean(maxdist);
mdFt = meanDist*3.28;
stdDist = std(maxdist);
stdDft = stdDist*3.28;

meanDistIsp = mean(maxdistIsp);
mdFtIsp = meanDistIsp*3.28;
stdDistIsp = std(maxdistIsp);
stdDftIsp = stdDistIsp*3.28;


%loop over all the data and plot stuff nicely 
figure
hold all
maxMaxDist = max(maxdist);
maxMaxHeight = max(maxheight);

maxMaxDistIsp = max(maxdistIsp);
maxMaxHeightIsp = max(maxheight);

totMaxDist  =max(max(maxMaxDistIsp,maxMaxDist),thermo_maxdist);
totMaxHeight = max(max(maxMaxHeightIsp,maxMaxHeight),thermo_maxheight);
axis([0,totMaxDist,-10,10,0,totMaxHeight])
for i = 1:nCases
    
    %set data after its passed the ground to NaN's so they don't show up on
    %the plot
    distVec = sqrt(dat(i).x(:,1).^2+dat(i).x(:,3).^2);
    distVecIsp = sqrt(datIsp(i).x(:,1).^2+datIsp(i).x(:,3).^2);
    
    [~,mDistInd] = min(abs(distVec - maxdist(i)));
    [~,mDistIndIsp] = min(abs(distVecIsp - maxdistIsp(i)));
    
    %plot everything
    phandle((i-1)*2+1) = plot3(dat(i).x(1:mDistInd,1),...
        dat(i).x(1:mDistInd,3),dat(i).x(1:mDistInd,2),'b');
    ltitle{(i-1)*2+1} = ['Thrust Interpolation ' num2str(i)];
    
    
    phandle((i-1)*2+2) = plot3(datIsp(i).x(1:mDistIndIsp,1),...
        datIsp(i).x(1:mDistIndIsp,3),datIsp(i).x(1:mDistIndIsp,2),'r');
    ltitle{(i-1)*2+2} = ['\Delta V via I_{sp}' num2str(i)];
end
%plot the mean distance
meanplot = [mean(maxdist),0,0];
phandle(nCases*2+1) = plot3(meanplot(1),meanplot(2),meanplot(3),'g*','MarkerSize',20,'LineWidth',2);
ltitle{nCases*2+1} = 'Mean Dist Val Interp';

meanplot = [mean(maxdistIsp),0,0];
phandle(nCases*2+2) = plot3(meanplot(1),meanplot(2),meanplot(3),'g.','MarkerSize',30,'LineWidth',3);
ltitle{nCases*2+2} = 'Mean Dist Val Isp';


% plot mean Isp trajectory
distVec = sqrt(meanIsp_x(:,1).^2+meanIsp_x(:,3).^2);
[~,mDistInd] = min(abs(distVec - meanIsp_maxdist));

phandle(nCases*2+3) = plot3(meanIsp_x(1:mDistInd,1),...
        meanIsp_x(1:mDistInd,3),meanIsp_x(1:mDistInd,2),'m','LineWidth',linewidth);
ltitle{nCases*2+3} = 'Mean I_{sp} Trajectory';

%plot thermo model trajectory
distVec = sqrt(thermo_x(:,1).^2+thermo_x(:,3).^2);
[~,mDistInd] = min(abs(distVec - thermo_maxdist));

phandle(nCases*2+4) = plot3(thermo_x(1:mDistInd,1),...
    thermo_x(1:mDistInd,3),thermo_x(1:mDistInd,2),'g','LineWidth',linewidth);
ltitle{nCases*2+4} = 'Thermo Model Trajectory';

% plot standard dev marks for interp
xplot = [meanDist+stdDist,meanDist+stdDist;meanDist-stdDist,meanDist-stdDist];
yplot = [0,totMaxHeight;0,totMaxHeight];
zplot = [0,0;0,0];
phandle(nCases*2+5) = plot3(xplot(1,:),zplot(1,:),yplot(1,:),'--c','LineWidth',linewidth);
phandle(nCases*2+6) = plot3(xplot(2,:),zplot(2,:),yplot(2,:),'--c','LineWidth',linewidth);
ltitle{nCases*2+5} = 'One Standard Deviation from Mean Interpolation Distance';
ltitle{nCases*2+6} = 'One Standard Deviation from Mean Interpolation Distance';

% plot standard dev marks for isp
xplot = [meanDistIsp+stdDistIsp,meanDistIsp+stdDistIsp;meanDistIsp-stdDistIsp,meanDistIsp-stdDistIsp];
yplot = [0,totMaxHeight;0,totMaxHeight];
zplot = [0,0;0,0];
phandle(nCases*2+7) = plot3(xplot(1,:),zplot(1,:),yplot(1,:),'--b','LineWidth',linewidth);
phandle(nCases*2+8) = plot3(xplot(2,:),zplot(2,:),yplot(2,:),'--b','LineWidth',linewidth);
ltitle{nCases*2+7} = 'One Standard Deviation from Mean I_{sp} Distance';
ltitle{nCases*2+8} = 'One Standard Deviation from Mean I_{sp} Distance';


% spit out the legends that we desire
includeInds = [1:2,nCases*2+1,nCases*2+2,nCases*2+3,nCases*2+4,nCases*2+5,nCases*2+7];
for i = 1:length(includeInds)
    shortPhandle(i) = phandle(includeInds(i));
    shortltitle{i} = ltitle{includeInds(i)};
end
legend(shortPhandle,shortltitle)
xlabel('x position')
ylabel('z position')
zlabel('y position')
grid on
% view([-28 23])
view([0,0])
    
% print the run time
% ttime = sum(loadtime) + sum(simTime);
% fprintf('\n\nTotal run time is %2.1f s\n',ttime)



fprintf('\nFrom Thrust Interpolation:\nMean Dist = %2.1fm (%3.1f ft), while the std dist = %2.1fm (%3.1f ft)\n',meanDist,mdFt,stdDist,stdDft)

fprintf('\nFrom Instantaneous Delta-V via Isp:\nMean Dist = %2.1fm (%3.1f ft), while the std dist = %2.1fm (%3.1f ft)\n',meanDistIsp,mdFtIsp,stdDistIsp,stdDftIsp)

thermo_maxdistFt = thermo_maxdist*3.28;
fprintf('\nFrom Thermo Model:\nDistance Traveled = %2.1fm (%3.1f ft)\n',thermo_maxdist,thermo_maxdistFt);

%throw out thigns +- 1 standard deviation
nDevs = 1;
indInt = 0;
indIsp = 0;
for i = 1:nCases
    if ~((maxdist(i) < meanDist - nDevs*stdDist) || (maxdist(i) > meanDist + nDevs*stdDist))
        indInt = indInt+1;
        maxDistsFiltered(indInt) = maxdist(i);
    end
        
    if ~((maxdistIsp(i) < meanDistIsp - nDevs*stdDistIsp) || (maxdistIsp(i) > meanDistIsp + nDevs*stdDistIsp))
        indIsp = indIsp+1;
        maxDistsFilteredIsp(indIsp) = maxdistIsp(i);
    end
    
end

%calc new means and stuffs
meanDistFiltered = mean(maxDistsFiltered);
mdfFt = meanDistFiltered*3.28;
stdDistFilt = std(maxDistsFiltered);
stdDistFiltFt = stdDistFilt*3.28;

meanDistFilteredIsp = mean(maxDistsFilteredIsp);
mdfFtIsp = meanDistFilteredIsp*3.28;
stdDistFiltIsp = std(maxDistsFilteredIsp);
stdDistFiltFtIsp = stdDistFiltIsp*3.28;


fprintf('\n\n Ignoring values more than %2.0f standard deviation from the mean\n',nDevs)
fprintf('\nFrom Thrust Interpolation:\nMean Dist = %2.1fm (%3.1f ft), while the std dist = %2.1fm (%3.1f ft)\n',meanDistFiltered,mdfFt,stdDistFilt,stdDistFiltFt)
fprintf('\nFrom Instantaneous Delta-V via Isp:\nMean Dist = %2.1fm (%3.1f ft), while the std dist = %2.1fm (%3.1f ft)\n',meanDistFilteredIsp,mdfFtIsp,stdDistFiltIsp,stdDistFiltFtIsp)


% plot scatter of Isp vs interp data
xplot = [min(maxdist),max(maxdist)]; yplot = [min(maxdistIsp),max(maxdistIsp)];
% calc slope for best fit line and plot as well...
Pvals = polyfit(maxdist,maxdistIsp,1);
xfit = [min(maxdist),max(maxdist)]; yfit = polyval(Pvals,xfit);
figure
plot(maxdist,maxdistIsp,'*',xplot,xplot,xfit,yfit)
xlabel('Interpolation Final Distance')
legend('Data','1-1 Line','Best Fit Line')
ylabel('Isp Final Distance')
tstring = ['I_{sp} distance = ' num2str(Pvals(1)) '*Interp Distance + ' num2str(Pvals(2))];
title(tstring)
grid on


% plot burnout velocities
figure
xthermo = [0,nCases];
ythermo = [Vburnoutthermo,Vburnoutthermo];

xIspMean = xthermo;
yIspMean = [VburnoutIspMean,VburnoutIspMean];

xInterpMean = xthermo;
yInterpMean = mean(VburnoutInt)*[1,1];

hold all
vbH(1) = plot(xthermo,ythermo,'g');
lstr{1} = 'Thermo Model';

vbH(2) = plot(xIspMean,yIspMean,'b');
lstr{2} = 'Mean Isp Model';

vbH(3) = plot(VburnoutIsp,'b*');
lstr{3} = 'Isp Model';

vbH(4) = plot(VburnoutInt,'r*');
lstr{4} = 'Interpolation Model';

vbH(5) = plot(xInterpMean,yInterpMean,'r');
lstr{5} = 'Interpolation Mean';

xlabel('Static Test Case')
ylabel('Burnout Velocity (m/s)')
legend(vbH,lstr)
grid on
if ispBurnoutFlag == 1 % burnout velocity from isp model simply = deltaV
    tstring = 'Isp Model Burnout Velocity Equal to Delta-V from Rocket Eqn';
else % burnout velocity from isp model equal to velocity at 1/4 sec after launch
    % simulating time to burnout
    tstring = 'Isp Model Burnout Vel Equal to Velocity at 1/4 sec after launch';
end
title(tstring);
    


% plot thrusts
figure
hold on

maxThrust = 0;
for i = 1:nCases
    
    [times,thrusts] = loadThrustData(filenames{i},plotThings);
    
    ph(i) = plot(times,thrusts,'b');
    
    
    if max(thrusts) > maxThrust
        maxThrust = max(thrusts);
    end
end

ph(nCases+1) = plot(thermo_t,thrust_thermo,'r','LineWidth',linewidth);
maxThrust = (max(thrust_thermo) > maxThrust)*max(thrust_thermo) + ...
    (max(thrust_thermo) < maxThrust)*maxThrust;
xlabel('Time (sec)')
ylabel('Thrust (N)')
legend(ph(nCases:nCases+1),{'Static Test Data','Thermo Model'})
axis([0,.3,0,1.2*maxThrust])
grid on
    